/*
 * rca_robot.cpp
 * nav_recip_rca
 *
 *
 * Copyright (c) 2008-2012, University of North Carolina at Chapel Hill
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of the University of North Carolina at Chapel Hill nor
 *     the names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 *     Dept. of Computer Science
 *     201 S. Columbia St.
 *     Frederick P. Brooks, Jr. Computer Science Bldg.
 *     Chapel Hill, N.C. 27599-3175
 *     United States of America
 *
 *     http://gamma.cs.unc.edu/ROS/
 */

#include <cmath>
#include <vector>
#include <ros/assert.h>
#include <nav_recip_rca/rca_robot.h>

namespace nav_recip
{
// Compute the new linear velocity of this RCARobot that avoids collisions with
// its neighbors.
bool RCARobot::computeNewVelocity(float time_step)
{
  computeLines(time_step);

  return solveLinearProgram();
}

// Compute the line constraints bounding collision-avoiding velocities.
void RCARobot::computeLines(float time_step)
{
  ROS_ASSERT(time_horizon_ > 0.0f);

  lines_.reserve(neighbors_.size());
  lines_.clear();

  const float inverse_time_horizon = 1.0f / time_horizon_;
  const float inverse_time_step = 1.0f / time_step;

  for (ConstRobotIterator it = neighbors_.begin(); it != neighbors_.end(); ++it) {
    const Vector2 relative_velocity = (*it)->velocity_ - velocity_;
    const Vector2 relative_position = (*it)->position_ - position_;
    const float distance_squared = relative_position.length2();
    const float combined_radius = (*it)->radius_ + radius_;
    const float inverse_cut_off_radius = FSel(distance_squared - combined_radius * combined_radius,
                                              inverse_time_horizon, inverse_time_step);
    Vector2 line_direction = relative_velocity + inverse_cut_off_radius * relative_position;
    const float length_squared = line_direction.length2();
    const float dot_product = line_direction.dot(relative_position);
    
    line_direction.normalize();

    Vector2 line_position = inverse_cut_off_radius * (relative_position - combined_radius * line_direction);
    
    line_direction.setOrthogonal();
    
    if ((distance_squared > combined_radius * combined_radius)
        && !(dot_product > 0.0f && dot_product * dot_product > combined_radius * combined_radius * length_squared)) {
      const float leg = std::sqrt(distance_squared - combined_radius * combined_radius);
        
      line_direction = combined_radius * relative_position.orthogonal()
          + relative_position * FSel(relative_velocity.cross(relative_position), leg, -leg);
      line_direction.normalize();
      line_position = -relative_velocity.dot(line_direction) * line_direction;
    }
    
    line_position += relative_velocity;
    line_position *= 0.5f;
    line_position += velocity_; 

    lines_.push_back(Line(line_position, line_direction));
  }
}

// Solve a linear program subject to line constraints and a circle constraint.
bool RCARobot::solveLinearProgram()
{
  size_t failure_line_no = solveLinearProgram2D(lines_, preferred_velocity_, false);

  if (failure_line_no < lines_.size()) {
    solveLinearProgram3D(failure_line_no);

    return false;
  }

  return true;
}

// Solve a 1D linear program on a given line subject to given line constraints
// and a given circle constraint.
bool RCARobot::solveLinearProgram1D(const LineList& lines, size_t line_no, const Vector2& preferred_velocity,
                                    bool optimize_direction)
{
  const float dot_product = lines[line_no].position_.dot(lines[line_no].direction_);
  const float discriminant = dot_product * dot_product + max_speed_ * max_speed_ - lines[line_no].position_.length2();

  if (discriminant < 0.0f) {
    // Max speed circle constaint fully invalidates line line_no.
    return false;
  }

  const float sqrt_discriminant = std::sqrt(discriminant);
  float t_left = -dot_product - sqrt_discriminant;
  float t_right = -dot_product + sqrt_discriminant;

  for (LineConstIterator it = lines.begin(); it != lines.begin() + static_cast<ptrdiff_t>(line_no); ++it) {
    const float denominator = lines[line_no].direction_.cross((*it).direction_);
    const float numerator = (*it).direction_.cross(lines[line_no].position_ - (*it).position_);

    if (NAV_RECIP_UNLIKELY(denominator > -NAV_RECIP_EPSILON && denominator < NAV_RECIP_EPSILON)) {
      // Lines line_no and i are parallel.
      if (numerator < 0.0f) {
        return false;
      }

      continue;
    }

    const float t_temp = numerator / denominator;

    t_right = FSel(denominator, FSel(t_right - t_temp, t_temp, t_right), t_right);
    t_left = FSel(denominator, t_left, FSel(t_left - t_temp, t_left, t_temp));

    if (t_left > t_right) {
      return false;
    }
  }

  float t = 0.0f;

  if (optimize_direction) {
    // Optimize direction.
    t = FSel(preferred_velocity.dot(lines[line_no].direction_), t_right, t_left);
  }
  else {
    // Optimize closest point.
    t = lines[line_no].direction_.dot(preferred_velocity - lines[line_no].position_);
    t = FSel(t - t_left, FSel(t - t_right, t_right, t), t_left);
  }

  new_velocity_ = lines[line_no].position_ + t * lines[line_no].direction_;

  return true;
}

// Solve a 2D linear program subject to given line constraints and a given
// circle constraint.
size_t RCARobot::solveLinearProgram2D(const LineList& lines, const Vector2& preferred_velocity, bool optimize_direction)
{
  // Optimize closest point and inside circle.
  new_velocity_ = preferred_velocity;

  if (optimize_direction) {
    // Optimize direction. The preferred velocity is of unit length.
    new_velocity_ *= max_speed_;
  }
  else {
    const float new_speed_squared = new_velocity_.length2();

    if (new_speed_squared > max_speed_ * max_speed_) {
      // Optimize closest point and outside circle.
      new_velocity_ *= max_speed_ / std::sqrt(new_speed_squared);
    }
  }

  for (size_t i = 0; i < lines.size(); ++i) {
    if (lines[i].direction_.cross(lines[i].position_ - new_velocity_) > 0.0f) {
      // New velocity does not satisfy constraint i. Compute another new velocity.
      const Vector2 temp_new_velocity = new_velocity_;

      if (!solveLinearProgram1D(lines, i, preferred_velocity, optimize_direction)) {
        new_velocity_ = temp_new_velocity;

        return i;
      }
    }
  }

  return lines.size();
}

// Solve a 3D linear program subject to line constraints and a given circle
// constraint.
void RCARobot::solveLinearProgram3D(size_t line_no)
{
  LineList projected_lines;
  projected_lines.reserve(lines_.size());

  float distance = 0.0f;

  for (size_t i = line_no; i < lines_.size(); ++i) {
    if (lines_[i].direction_.cross(lines_[i].position_ - new_velocity_) > distance) {
      // Result does not satisfy line constraint i.
      projected_lines.clear();

      for (size_t j = 0; j < i; ++j) {
        Line line;

        float determinant = lines_[i].direction_.cross(lines_[j].direction_);

        if (NAV_RECIP_UNLIKELY(determinant > -NAV_RECIP_EPSILON && determinant < NAV_RECIP_EPSILON)) {
          // Lines i and j are parallel.
          if (lines_[i].direction_.dot(lines_[j].direction_) > 0.0f) {
            // Lines i and j point in the same direction.
            continue;
          }

          // Lines i and j point in opposite directions.
          line.position_ = 0.5f * (lines_[i].position_ + lines_[j].position_);

        }
        else {
          line.position_ = lines_[i].position_ + (lines_[j].direction_.cross(lines_[i].position_ - lines_[j].position_) / determinant) * lines_[i].direction_;
        }

        line.direction_ = (lines_[j].direction_ - lines_[i].direction_).normalized();
        projected_lines.push_back(line);
      }

      const Vector2 temp_new_velocity = new_velocity_;

      if (NAV_RECIP_UNLIKELY(solveLinearProgram2D(projected_lines, lines_[i].direction_.orthogonal(), true) < projected_lines.size())) {
        // Should not happen. The new velocity is already in the feasible region of the linear program. Failure is due
        // to a small floating point error so the current new velocity is kept.
        new_velocity_ = temp_new_velocity;
      }

      distance = lines_[i].direction_.cross(lines_[i].position_ - new_velocity_);
    }
  }
}
}
